/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*         Mike Phillips (put the planner in its own thread)
*********************************************************************/
#include <move_base/move_base.h>
#include <move_base_msgs/RecoveryStatus.h>
#include <cmath>
#include <boost/algorithm/string.hpp>
#include <boost/thread.hpp>
#include <geometry_msgs/Twist.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

namespace move_base
{

MoveBase::MoveBase(tf2_ros::Buffer &tf) :
        tf_(tf),
        as_(NULL),
        planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
        bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),     // 加载了baseGlobalPlanner的类库
        blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),      // 加载了baseLocalPlanner的类库
        recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), // 加载了recoveryBehaviour的类库
        planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
        runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false)
{
    // as_指向action服务器，当执行as_->start()时调用MoveBase::executeCb函数
    as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", [this](auto &goal) { executeCb(goal); }, false);
    ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;
    recovery_trigger_ = PLANNING_R;
    // 设置move_base_node中的全局参数，如果在yaml文件中没有设置对应的参数，则默认使用param函数中最后的参数
    //get some parameters that will be global to the move base node
    std::string global_planner, local_planner;
    private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
    private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
    private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
    private_nh.param("planner_frequency", planner_frequency_, 0.0);        // 全局路径规划器循环速率
    private_nh.param("controller_frequency", controller_frequency_, 20.0); // 控制器向机器人底盘 cmd_vel 发送命令的频率
    private_nh.param("planner_patience", planner_patience_, 5.0);          // 路径规划器等待有效控制命令的时间
    private_nh.param("controller_patience", controller_patience_, 15.0);   // 控制器等待有效控制命令的时间
    private_nh.param("max_planning_retries", max_planning_retries_, -1);   // 执行恢复行为之前允许计划重试的次数,-1.0对应于无限次重试。
    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);    // 震荡时间： 多久判断一次
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);  // 震荡距离
    // parameters of make_plan service
    private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true); // 路径规划是否清除代价地图
    private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true); // 路径规划是否添加无法达到的目标点
    // set up plan triple buffer 初始化三个plan的“缓冲池”数组
    planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
    // set up the planner's thread 新建planner线程，入口函数为MoveBase::planThread
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
    // for commanding the base 下发命令给基座
    vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0);
    // movebase命名空间下的句柄
    ros::NodeHandle action_nh("move_base");
    // 发布MoveBaseActionGoal消息到/move_base/goal话题上
    action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
    recovery_status_pub_ = action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);
    // we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
    // they won't get any useful information back about its status,
    // but this is useful for tools like nav_view and rviz
    //发布geometry_msgs::PoseStamped消息到/move_base_simple/goal话题上，回调函数goalCB会处理在/move_base_simple/goal话题上接收到的消息
    //供nav_view和rviz等仿真工具使用
    ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, [this](auto& goal){ goalCB(goal); });
    // we'll assume the radius of the robot to be consistent with what's specified for the costmaps
    // 加载代价地图的参数（内切、外接、清理半径等），假设机器人的半径和costmap规定的一致
    private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
    private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
    private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
    private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
    // 一些标志位的设置
    private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);                // 是否停用节点的costmap
    private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true); // 是否启用 move_base 恢复行为以尝试清除空间
    private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true); // 是否采用原地旋转的运动方式清理空间
    // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
    planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
    // 停止更新成本图，但传感器数据仍然通过线路传入
    planner_costmap_ros_->pause();
    // initialize the global planner 初始化全局规划器
    try
    {
        planner_ = bgp_loader_.createInstance(global_planner);
        planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
    }
    catch (const pluginlib::PluginlibException &ex)
    {
        ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s",
                global_planner.c_str(), ex.what());
        exit(1);
    }
    // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
    controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
    // 停止更新成本图，但传感器数据仍然通过线路传入
    controller_costmap_ros_->pause();
    // create a local planner 创建局部规划器
    try
    {
        tc_ = blp_loader_.createInstance(local_planner);
        ROS_INFO("Created local_planner %s", local_planner.c_str());
        tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
    }
    catch (const pluginlib::PluginlibException &ex)
    {
        ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s",
                local_planner.c_str(), ex.what());
        exit(1);
    }
    // Start actively updating costmaps based on sensor data
    planner_costmap_ros_->start();
    controller_costmap_ros_->start();
    // advertise a service for getting a plan 允许外部用户从 move_base 请求给定姿势的计划，而不会导致 move_base 执行该计划。
    make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
    // advertise a service for clearing the costmaps 清除代价地图服务clearCostmapsService分别清楚全局代价地图和局部代价地图
    clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
    // if we shutdown our costmaps when we're deactivated... we'll do that now
    if (shutdown_costmaps_)
    {
        ROS_DEBUG_NAMED("move_base", "Stopping costmaps initially");
        planner_costmap_ros_->stop();
        controller_costmap_ros_->stop();
    }
    // load any user specified recovery behaviors, and if that fails load the defaults
    // 如果加载默认值失败, 加载任何用户指定恢复行为
    if (!loadRecoveryBehaviors(private_nh)) loadDefaultRecoveryBehaviors();
    // initially, we'll need to make a plan 首先，我们需要制定一个计划
    state_ = PLANNING;
    // we'll start executing recovery behaviors at the beginning of our list 我们将在列表的开头开始执行恢复行为
    recovery_index_ = 0;
    // we're all set up now so we can start the action server 我们现在都设置好了，可以启动动作服务器了
    as_->start();
    dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
    dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb =
            [this](auto &config, auto level) { reconfigureCB(config, level); };
    dsrv_->setCallback(cb);
}

void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level)
{
    boost::recursive_mutex::scoped_lock l(configuration_mutex_);
    // The first time we're called, we just want to make sure we have the
    // original configuration
    if (!setup_)
    {
        last_config_ = config;
        default_config_ = config;
        setup_ = true;
        return;
    }
    if (config.restore_defaults)
    {
        config = default_config_;
        //if someone sets restore defaults on the parameter server, prevent looping
        config.restore_defaults = false;
    }
    if (planner_frequency_ != config.planner_frequency)
    {
        planner_frequency_ = config.planner_frequency;
        p_freq_change_ = true;
    }
    if (controller_frequency_ != config.controller_frequency)
    {
        controller_frequency_ = config.controller_frequency;
        c_freq_change_ = true;
    }
    planner_patience_ = config.planner_patience;
    controller_patience_ = config.controller_patience;
    max_planning_retries_ = config.max_planning_retries;
    conservative_reset_dist_ = config.conservative_reset_dist;
    recovery_behavior_enabled_ = config.recovery_behavior_enabled;
    clearing_rotation_allowed_ = config.clearing_rotation_allowed;
    shutdown_costmaps_ = config.shutdown_costmaps;
    oscillation_timeout_ = config.oscillation_timeout;
    oscillation_distance_ = config.oscillation_distance;
    // 是否更换全局路径规划器
    if (config.base_global_planner != last_config_.base_global_planner)
    {
        boost::shared_ptr <nav_core::BaseGlobalPlanner> old_planner = planner_;
        // initialize the global planner
        ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
        try
        {
            planner_ = bgp_loader_.createInstance(config.base_global_planner);
            // wait for the current planner to finish planning
            boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
            // Clean up before initializing the new planner
            planner_plan_->clear();
            latest_plan_->clear();
            controller_plan_->clear();
            resetState();
            planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);
            lock.unlock();
        }
        catch (const pluginlib::PluginlibException &ex)
        {
            ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
               containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
            planner_ = old_planner;
            config.base_global_planner = last_config_.base_global_planner;
        }
    }
    // 是否更换局部路径规划器
    if (config.base_local_planner != last_config_.base_local_planner)
    {
        boost::shared_ptr <nav_core::BaseLocalPlanner> old_planner = tc_;
        // create a local planner
        try
        {
            tc_ = blp_loader_.createInstance(config.base_local_planner);
            // Clean up before initializing the new planner
            planner_plan_->clear();
            latest_plan_->clear();
            controller_plan_->clear();
            resetState();
            tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
        }
        catch (const pluginlib::PluginlibException &ex)
        {
            ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
               containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
            tc_ = old_planner;
            config.base_local_planner = last_config_.base_local_planner;
        }
    }
    make_plan_clear_costmap_ = config.make_plan_clear_costmap;
    make_plan_add_unreachable_goal_ = config.make_plan_add_unreachable_goal;
    last_config_ = config;
}

void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal)
{
    ROS_DEBUG_NAMED("move_base",
                    "In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
    move_base_msgs::MoveBaseActionGoal action_goal;
    action_goal.header.stamp = ros::Time::now();
    action_goal.goal.target_pose = *goal;
    action_goal_pub_.publish(action_goal);
}

void MoveBase::clearCostmapWindows(double size_x, double size_y)
{
    geometry_msgs::PoseStamped global_pose;
    //clear the planner's costmap
    getRobotPose(global_pose, planner_costmap_ros_);
    std::vector <geometry_msgs::Point> clear_poly;
    double x = global_pose.pose.position.x;
    double y = global_pose.pose.position.y;
    geometry_msgs::Point pt;
    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);
    planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
    // clear the controller's costmap
    getRobotPose(global_pose, controller_costmap_ros_);
    clear_poly.clear();
    x = global_pose.pose.position.x;
    y = global_pose.pose.position.y;

    pt.x = x - size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y - size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x + size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    pt.x = x - size_x / 2;
    pt.y = y + size_y / 2;
    clear_poly.push_back(pt);

    controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
}

bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
{
    // clear the costmaps
    boost::unique_lock <costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
    boost::unique_lock <costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
    controller_costmap_ros_->resetLayers();
    planner_costmap_ros_->resetLayers();
    return true;
}

bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
{
    if (as_->isActive())
    {
        ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
        return false;
    }
    // make sure we have a costmap for our planner
    if (planner_costmap_ros_ == NULL)
    {
        ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
        return false;
    }
    geometry_msgs::PoseStamped start;
    // if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
    if (req.start.header.frame_id.empty())
    {
        geometry_msgs::PoseStamped global_pose;
        // 获取机器人位置
        if (!getRobotPose(global_pose, planner_costmap_ros_))
        {
            ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
            return false;
        }
        start = global_pose;
    }
    else start = req.start;
    if (make_plan_clear_costmap_)
    {
        // update the copy of the costmap the planner uses
        clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
    }
    // first try to make a plan to the exact desired goal
    std::vector <geometry_msgs::PoseStamped> global_plan;
    // 如果第一次规划不出路径
    if (!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty())
    {
        ROS_DEBUG_NAMED("move_base",
                        "Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
                        req.goal.pose.position.x, req.goal.pose.position.y);
        // search outwards for a feasible goal within the specified tolerance
        geometry_msgs::PoseStamped p;
        p = req.goal;
        bool found_legal = false;
        float resolution = planner_costmap_ros_->getCostmap()->getResolution();
        // 探索增量
        float search_increment = resolution * 3.0;
        if (req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
        for (float max_offset = search_increment;
             max_offset <= req.tolerance && !found_legal; max_offset += search_increment)
        {
            for (float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment)
            {
                for (float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment)
                {
                    // don't search again inside the current outer layer
                    // 不要在当前的外层内部再次搜索
                    if (x_offset < max_offset - 1e-9 && y_offset < max_offset - 1e-9) continue;
                    // search to both sides of the desired goal
                    // 搜索到期望目标的两边
                    for (float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0)
                    {
                        // if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
                        // 如果其中一个偏移量为0，-1*0仍然是0(所以去掉两个中的一个)
                        if (y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;
                        for (float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0)
                        {
                            if (x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;
                            p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
                            p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;
                            if (planner_->makePlan(start, p, global_plan))
                            {
                                if (!global_plan.empty())
                                {
                                    if (make_plan_add_unreachable_goal_)
                                    {
                                        // adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
                                        // (the reachable goal should have been added by the global planner)
                                        global_plan.push_back(req.goal);
                                    }
                                    found_legal = true;
                                    ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)",
                                                    p.pose.position.x, p.pose.position.y);
                                    break;
                                }
                            }
                            else
                            {
                                ROS_DEBUG_NAMED("move_base", "Failed to find a plan to point (%.2f, %.2f)",
                                                p.pose.position.x, p.pose.position.y);
                            }
                        }
                    }
                }
            }
        }
    }
    //copy the plan into a message to send out
    resp.plan.poses.resize(global_plan.size());
    for (unsigned int i = 0; i < global_plan.size(); ++i)
    {
        resp.plan.poses[i] = global_plan[i];
    }
    return true;
}

MoveBase::~MoveBase()
{
    recovery_behaviors_.clear();
    delete dsrv_;
    delete as_;
    delete planner_costmap_ros_;
    delete controller_costmap_ros_;
    planner_thread_->interrupt();
    planner_thread_->join();
    delete planner_thread_;
    delete planner_plan_;
    delete latest_plan_;
    delete controller_plan_;
    planner_.reset();
    tc_.reset();
}

bool MoveBase::makePlan(const geometry_msgs::PoseStamped &goal, std::vector <geometry_msgs::PoseStamped> &plan)
{
    boost::unique_lock <costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex()));
    //make sure to set the plan to be empty initially 清空路径
    plan.clear();
    //since this gets called on handle activate 如果没有全局代价地图，返回false
    if (planner_costmap_ros_ == NULL)
    {
        ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan");
        return false;
    }
    //get the starting pose of the robot 获取机器人的起始位姿
    geometry_msgs::PoseStamped global_pose;
    if (!getRobotPose(global_pose, planner_costmap_ros_))
    {
        ROS_WARN("Unable to get starting pose of robot, unable to create global plan");
        return false;
    }
    const geometry_msgs::PoseStamped &start = global_pose;
    // 如果全局规划器失败了或者返回了一个零长度的路径，规划失败
    //if the planner fails or returns a zero length plan, planning failed
    if (!planner_->makePlan(start, goal, plan) || plan.empty())
    {
        ROS_DEBUG_NAMED("move_base", "Failed to find a  plan to point (%.2f, %.2f)", goal.pose.position.x,
                        goal.pose.position.y);
        return false;
    }
    return true;
}

void MoveBase::publishZeroVelocity()
{
    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.0;
    cmd_vel.linear.y = 0.0;
    cmd_vel.angular.z = 0.0;
    vel_pub_.publish(cmd_vel);
}

bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion &q)
{
    // 先检查四元数的值有没有非法值(nan)或者无穷大的值(inf)
    //first we need to check if the quaternion has nan's or infs
    if (!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w))
    {
        ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
        return false;
    }
    tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);
    // 接下来检查四元数的长度是不是趋于零
    // next, we need to check if the length of the quaternion is close to zero
    if (tf_q.length2() < 1e-6)
    {
        ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
        return false;
    }
    // 归一化四元数
    // next, we'll normalize the quaternion and check that it transforms the vertical vector correctly
    tf_q.normalize();
    tf2::Vector3 up(0, 0, 1);
    double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
    if (fabs(dot - 1) > 1e-3)
    {
        ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
        return false;
    }
    return true;
}
// 将目标点转换到全局坐标系
geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg)
{
    std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
    geometry_msgs::PoseStamped global_pose;
    try
    {
        tf_.transform(goal_pose_msg, global_pose, global_frame);
    }
    catch (tf2::TransformException &ex)
    {
        ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
                 goal_pose_msg.header.frame_id.c_str(), global_frame.c_str(), ex.what());
        return goal_pose_msg;
    }
    return global_pose;
}

void MoveBase::wakePlanner(const ros::TimerEvent &event)
{
    // we have slept long enough for rate
    planner_cond_.notify_one();
}

void MoveBase::planThread()
{
    ROS_DEBUG_NAMED("move_base_plan_thread", "Starting planner thread...");
    ros::NodeHandle n;
    ros::Timer timer;
    bool wait_for_wake = false;
    boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
    while (n.ok())
    {
        // 确认是否要运行路径规划器(这里已经加锁)
        // check if we should run the planner (the mutex is locked)
        while (wait_for_wake || !runPlanner_)
        {
            // 暂时关闭路径规划线程
            // if we should not be running the planner then suspend this thread
            ROS_DEBUG_NAMED("move_base_plan_thread", "Planner thread is suspending");
            // 注意planner_cond_.wait(lock)是在等待条件满足。
            // 如果条件不满足，则释放锁，将线程置为waiting状态，继续等待；
            // 如果条件满足，则重新获取锁，结束wait，继续向下执行
            planner_cond_.wait(lock);
            wait_for_wake = false;
        }
        ros::Time start_time = ros::Time::now();
        // 开始规划了，复制路径规划器的目标点(注意这里在上次循环中加锁了)，然后在这次解锁。
        // time to plan! get a copy of the goal and unlock the mutex
        geometry_msgs::PoseStamped temp_goal = planner_goal_;
        lock.unlock();
        ROS_DEBUG_NAMED("move_base_plan_thread", "Planning...");
        // run planner 运行路径规划器，它的主要函数是makePlan
        planner_plan_->clear();
        if (n.ok() && makePlan(temp_goal, *planner_plan_))
        {
            // 如果规划出路径则更新相应路径，并将state_设成CONTROLLING状态
            ROS_DEBUG_NAMED("move_base_plan_thread", "Got Plan with %zu points!", planner_plan_->size());
            // pointer swap the plans under mutex (the controller will pull from latest_plan_)
            std::vector <geometry_msgs::PoseStamped> *temp_plan = planner_plan_;
            lock.lock();
            planner_plan_ = latest_plan_;
            // 将最新的全局路径放到latest_plan_中，
            // 其在MoveBase::executeCycle中被传递到controller_plan_中，用锁来进行同步
            latest_plan_ = temp_plan;
            last_valid_plan_ = ros::Time::now();
            planning_retries_ = 0;
            new_global_plan_ = true;
            ROS_DEBUG_NAMED("move_base_plan_thread", "Generated a plan from the base_global_planner");
            // make sure we only start the controller if we still haven't reached the goal 如果没有到达目标点，进入CONTROLLING状态
            if (runPlanner_) state_ = CONTROLLING;
            if (planner_frequency_ <= 0) runPlanner_ = false;
            lock.unlock();
        }
            // if we didn't get a plan and we are in the planning state (the robot isn't moving) 如果没有规划出路径同时state_为PLANNING状态
        else if (state_ == PLANNING)
        {
            ROS_DEBUG_NAMED("move_base_plan_thread", "No Plan...");
            ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
            // check if we've tried to make a plan for over our time limit or our maximum number of retries
            // issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_ is negative (the default),
            // it is just ignored and we have the same behavior as ever
            // 判断是否超过最大规划周期或者规划次数
            // 如果是则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划
            lock.lock();
            planning_retries_++;
            // 仅在MoveBase::executeCb及其调用的MoveBase::executeCycle
            // 或者重置状态时会被设置为PLANNING，一般是刚获得新目标，
            // 或者得到路径但计算不出下一步控制时重新进行路径规划
            if (runPlanner_ && (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_)))
            {
                // we'll move into our obstacle clearing mode 进入障碍物清理模式
                state_ = CLEARING;
                runPlanner_ = false;  // proper solution for issue #523
                publishZeroVelocity();
                recovery_trigger_ = PLANNING_R;
            }
            lock.unlock();
        }
        //take the mutex for the next iteration 加锁，下次循环中解锁
        lock.lock();
        //setup sleep interface if needed 如果还没到规划周期则定时器睡眠，在定时器中断中通过planner_cond_唤醒，这里规划周期为0
        if (planner_frequency_ > 0)
        {
            ros::Duration sleep_time = (start_time + ros::Duration(1.0 / planner_frequency_)) - ros::Time::now();
            if (sleep_time > ros::Duration(0.0))
            {
                wait_for_wake = true;
                timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
            }
        }
    }
}

// 控制的主要函数 geometry_msgs/PoseStamped target_pose
void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal)
{
    // 如果目标点朝向的四元数不合法，退出该函数
    if (!isQuaternionValid(move_base_goal->target_pose.pose.orientation))
    {
        as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
        return;
    }
    // 判断goal的有效性，统一转换到全局坐标系
    geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
    // 发布零速度
    publishZeroVelocity();
    // we have a goal so start the planner 现在我们有了目标点，开始路径规划
    boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
    planner_goal_ = goal;
    // 全局规划标志位设为真
    runPlanner_ = true;
    // 由于全局规划器线程绑定的函数plannerThread()里有planner_cond_对象的wait函数，在这里调用notify启动全局规划器线程，进行全局路径规划
    // 唤醒等待条件变量的一个线程：即调用planner_cond_.wait()的MoveBase::planThread()
    planner_cond_.notify_one();
    lock.unlock();
    current_goal_pub_.publish(goal);
    // 设置局部规划频率
    ros::Rate r(controller_frequency_);
    // 如果代价地图被关闭，则打开
    if (shutdown_costmaps_)
    {
        ROS_DEBUG_NAMED("move_base", "Starting up costmaps that were shut down previously");
        planner_costmap_ros_->start();
        controller_costmap_ros_->start();
    }
    // we want to make sure that we reset the last time we had a valid plan and control
    // 上一次的局部规划时间更新为当前时间
    last_valid_control_ = ros::Time::now();
    // 上一次的全局规划时间更新为当前时间
    last_valid_plan_ = ros::Time::now();
    // 上一次的震荡重置时间更新为当前时间
    last_oscillation_reset_ = ros::Time::now();
    // 对同一目标的全局规划次数记录归为0
    planning_retries_ = 0;
    ros::NodeHandle n;
    // 全局规划完成，接下来循环调用executeCycle函数来控制机器人进行局部规划，完成相应跟随。
    while (n.ok())
    {
        // 是否需要更改控制频率
        if (c_freq_change_)
        {
            ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
            r = ros::Rate(controller_frequency_);
            c_freq_change_ = false;
        }
        // step xx 如果action_server被抢占，两个情况:
        // case 1. 局部规划过程中有了新的目标
        // case 2. 收到取消任务的命令
        if (as_->isPreemptRequested())
        {
            // 上述的case 1 有没有新的目标点出现
            if (as_->isNewGoalAvailable())
            {
                // 有了新的可行目标后，新目标将会被执行，没有东西会被关闭
                //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
                move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
                if (!isQuaternionValid(new_goal.target_pose.pose.orientation))
                {
                    as_->setAborted(move_base_msgs::MoveBaseResult(),
                                    "Aborting on goal because it was sent with an invalid quaternion");
                    return;
                }
                goal = goalToGlobalFrame(new_goal.target_pose);
                // 为下个执行循环重置状态
                //we'll make sure that we reset our state for the next execution cycle
                recovery_index_ = 0;
                state_ = PLANNING;
                // 现在有新的目标点，唤醒路径规划线程
                //we have a new goal so make sure the planner is awake
                lock.lock();
                planner_goal_ = goal;
                runPlanner_ = true;
                planner_cond_.notify_one();
                lock.unlock();
                // 发布该目标点给rviz
                //publish the goal point to the visualizer
                ROS_DEBUG_NAMED("move_base", "move_base has received a goal of x: %.2f, y: %.2f",
                                goal.pose.position.x, goal.pose.position.y);
                // 把目标发给可视化平台
                current_goal_pub_.publish(goal);
                // 相关的计时变量也要更新
                //make sure to reset our timeouts and counters
                last_valid_control_ = ros::Time::now();
                last_valid_plan_ = ros::Time::now();
                last_oscillation_reset_ = ros::Time::now();
                planning_retries_ = 0;
            }
            else
            {
                // 上述的case 2： 如果任务被取消，重置导航各个部分的状态
                // if we've been preempted explicitly we need to shut things down
                resetState();
                //notify the ActionServer that we've successfully preempted
                ROS_DEBUG_NAMED("move_base", "Move base preempting the current goal");
                // 设置当前的 goal为被强占状态
                as_->setPreempted();
                // we'll actually return from execute after preempting
                // 我们会在抢占后从execute返回
                return;
            }
        }
        // step xx 如果action_server未被抢占或被抢占的if结构已执行完毕，接下来开始局部规划，调用executeCycle函数，并记录局部控制起始时间
        // 如果目标点的坐标系和全局地图的坐标系不相同，那就转换到世界坐标系，然后唤醒全局路径规划
        // we also want to check if we've changed global frames because we need to transform our goal pose
        if (goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID())
        {
            goal = goalToGlobalFrame(goal);
            // 进入planning状态，准备下个执行周期
            //we want to go back to the planning state for the next execution cycle
            recovery_index_ = 0;
            state_ = PLANNING;
            // 有了新的目标点，所以需要确认路径规划器是唤醒的
            //we have a new goal so make sure the planner is awake
            lock.lock();
            planner_goal_ = goal;
            runPlanner_ = true;
            planner_cond_.notify_one();
            lock.unlock();
            // 发布目标点给rviz
            //publish the goal point to the visualizer
            ROS_DEBUG_NAMED("move_base",
                            "The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f",
                            goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);
            current_goal_pub_.publish(goal);
            // 相关的计时变量也要更新
            //make sure to reset our timeouts and counters
            last_valid_control_ = ros::Time::now();
            last_valid_plan_ = ros::Time::now();
            last_oscillation_reset_ = ros::Time::now();
            planning_retries_ = 0;
        }
        // 开始为executeCycle函数计时，walltime计算的是客观的真实时间，而不是仿真里的时间
        //for timing that gives real time even in simulation
        ros::WallTime start = ros::WallTime::now();
        // 到达目标点的真正工作，控制机器人进行跟随;如果任务完成，则退出
        // the real work on pursuing a goal is done here; if we're done, then we'll return from execute
        if (executeCycle(goal)) return;
        //check if execution of the goal has completed in some way
        ros::WallDuration t_diff = ros::WallTime::now() - start;
        ROS_DEBUG_NAMED("move_base", "Full control cycle time: %.9f\n", t_diff.toSec());
        r.sleep();
        //make sure to sleep for the remainder of our cycle time
        if (r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
            ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds",
                     controller_frequency_, r.cycleTime().toSec());
    }
    // 唤醒路径规划的线程方便退出
    // wake up the planner thread so that it can exit cleanly
    lock.lock();
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();
    // 如果该节点被杀死，任务终止退出
    // if the node is killed then we'll abort and return
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");
}

double MoveBase::distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2)
{
    /* Return `sqrt(X*X + Y*Y)'.  */
    return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y);
}

// 执行一个控制循环
bool MoveBase::executeCycle(geometry_msgs::PoseStamped &goal)
{
    boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
    // 用于发布速度命令
    // we need to be able to publish velocity commands
    geometry_msgs::Twist cmd_vel;
    // 获取机器人的当前位姿
    // update feedback to correspond to our curent position
    geometry_msgs::PoseStamped global_pose;
    getRobotPose(global_pose, planner_costmap_ros_);
    const geometry_msgs::PoseStamped &current_position = global_pose;
    // 发布机器人当前位姿
    //push the feedback out
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);
    // 判断机器人是否走了足够的距离，如果是则重置震荡时间
    // check to see if we've moved far enough to reset our oscillation timeout
    if (distance(current_position, oscillation_pose_) >= oscillation_distance_)
    {
        last_oscillation_reset_ = ros::Time::now();
        oscillation_pose_ = current_position;
        // 长时间在小区域运动，重置恢复索引
        // if our last recovery was caused by oscillation, we want to reset the recovery index
        if (recovery_trigger_ == OSCILLATION_R) recovery_index_ = 0;
    }
    // 如果从costmap的buffer中观测数据不够新，则让机器人停止运动，退出函数
    // check that the observation buffers for the costmap are current, we don't want to drive blind
    if (!controller_costmap_ros_->isCurrent())
    {
        ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",
                 ros::this_node::getName().c_str());
        publishZeroVelocity();
        return false;
    }
    // 如果有了新的全局路径，把它传给局部规划器
    // if we have a new plan then grab it and give it to the controller
    if (new_global_plan_)
    {
        // 新全局路径标志位置为false
        // make sure to set the new plan flag to false
        new_global_plan_ = false;
        ROS_DEBUG_NAMED("move_base", "Got a new plan...swap pointers");
        // 全局路径的指针交换，controller_plan_和latest_plan_
        // do a pointer swap under mutex
        std::vector <geometry_msgs::PoseStamped> *temp_plan = controller_plan_;
        boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
        controller_plan_ = latest_plan_;
        latest_plan_ = temp_plan;
        lock.unlock();
        ROS_DEBUG_NAMED("move_base", "pointers swapped!");
        // tc是局部路径
        if (!tc_->setPlan(*controller_plan_))
        {
            // ABORT and SHUTDOWN COSTMAPS
            ROS_ERROR("Failed to pass global plan to the controller, aborting.");
            resetState();
            // disable the planner thread 关闭全局规划线程
            lock.lock();
            runPlanner_ = false;
            lock.unlock();
            as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
            return true;
        }
        // make sure to reset recovery_index_ since we were able to find a valid plan
        if (recovery_trigger_ == PLANNING_R) recovery_index_ = 0;
    }
    // the move_base state machine, handles the control logic for navigation 处理导航的控制逻辑
    switch (state_)
    {
        // if we are in a planning state, then we'll attempt to make a plan step x.x 如果是路径规划状态，计算路径
        case PLANNING:
            {
                // 加锁，唤醒路径规划器线程
                boost::recursive_mutex::scoped_lock lock(planner_mutex_);
                runPlanner_ = true;
                planner_cond_.notify_one();
            }
            ROS_DEBUG_NAMED("move_base", "Waiting for plan, in the planning state.");
            break;
            // step x.x 如果是控制状态，尝试计算出有效的下发速度命令
            // if we're controlling, we'll attempt to find valid velocity commands
        case CONTROLLING:
            ROS_DEBUG_NAMED("move_base", "In controlling state.");
            // check to see if we've reached our goal 是否到了目标点
            if (tc_->isGoalReached())
            {
                ROS_DEBUG_NAMED("move_base", "Goal reached!");
                resetState();
                //disable the planner thread 关闭路径规划线程
                boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
                runPlanner_ = false;
                lock.unlock();
                as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
                return true;
            }
            // step xx 检查是否满足震荡条件，
            // last_oscillation_reset_在一下几种情况都会被更新，
            // 1. 获得新目标
            // 2. 距离超过震荡距离（默认0.5）
            // 3. 进行recovery后
            // 4. 执行executeCb时，全局路径和局部路径有效时
            // 较长时间没有发生以上情况会触发震荡，防止长时间卡在一个地方。oscillation_timeout_默认为0。
            // check for an oscillation condition
            if (oscillation_timeout_ > 0.0 &&
                last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
            {
                publishZeroVelocity();
                state_ = CLEARING;
                // 机器人震荡
                recovery_trigger_ = OSCILLATION_R;
            }
            // step xx 如果一切正常，开始局部轨迹规划
            {
                boost::unique_lock <costmap_2d::Costmap2D::mutex_t> lock(
                        *(controller_costmap_ros_->getCostmap()->getMutex()));
                if (tc_->computeVelocityCommands(cmd_vel))
                {
                    // 如果局部轨迹规划成功
                    ROS_DEBUG_NAMED("move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
                                    cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
                    last_valid_control_ = ros::Time::now();
                    // 下发速度控制给机器人底座
                    // make sure that we send the velocity command to the base
                    vel_pub_.publish(cmd_vel);
                    if (recovery_trigger_ == CONTROLLING_R) recovery_index_ = 0;
                }
                else
                {
                    // 局部轨迹规划失败
                    ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");
                    // 判断控制命令的计算时间有没有超时
                    // check if we've tried to find a valid control for longer than our time limit
                    if (ros::Time::now() > last_valid_control_ + ros::Duration(controller_patience_))
                    {
                        // 进入障碍物清除模式(一种恢复行为)
                        // we'll move into our obstacle clearing mode
                        publishZeroVelocity();
                        state_ = CLEARING;
                        recovery_trigger_ = CONTROLLING_R;
                    }
                        // 如果没有超时，但是没有找到有效全局路径，启动路径规划器线程重新规划
                        // otherwise, if we can't find a valid control, we'll go back to planning
                    else
                    {
                        last_valid_plan_ = ros::Time::now();
                        planning_retries_ = 0;
                        state_ = PLANNING;
                        publishZeroVelocity();
                        // 使能全局规划的线程
                        // enable the planner thread in case it isn't running on a clock
                        boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
                        runPlanner_ = true;
                        planner_cond_.notify_one();
                        lock.unlock();
                    }
                }
            }
            break;
            // step x.x 用提供的恢复行为来清理空间，主要有三个恢复行为
            // we'll try to clear out space with any user-provided recovery behaviors
        case CLEARING:
            ROS_DEBUG_NAMED("move_base", "In clearing/recovery state");
            // 不管什么恢复行为，只要使能了，就唤醒执行
            // we'll invoke whatever recovery behavior we're currently on if they're enabled
            if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size())
            {
                ROS_DEBUG_NAMED("move_base_recovery", "Executing behavior %u of %zu", recovery_index_ + 1,
                                recovery_behaviors_.size());
                move_base_msgs::RecoveryStatus msg;
                msg.pose_stamped = current_position;
                msg.current_recovery_number = recovery_index_;
                msg.total_number_of_recoveries = recovery_behaviors_.size();
                msg.recovery_behavior_name = recovery_behavior_names_[recovery_index_];
                recovery_status_pub_.publish(msg);
                recovery_behaviors_[recovery_index_]->runBehavior();
                // 更新震荡的计时时间
                //we at least want to give the robot some time to stop oscillating after executing the behavior
                last_oscillation_reset_ = ros::Time::now();
                // 检查恢复行为是否有效
                //we'll check if the recovery behavior actually worked
                ROS_DEBUG_NAMED("move_base_recovery", "Going back to planning state");
                last_valid_plan_ = ros::Time::now();
                planning_retries_ = 0;
                state_ = PLANNING;
                // 更新到下个恢复行为的索引
                // update the index of the next recovery behavior that we'll try
                recovery_index_++;
            }
            else
            {
                ROS_DEBUG_NAMED("move_base_recovery",
                                "All recovery behaviors have failed, locking the planner and disabling it.");
                // disable the planner thread 关闭全局规划节点
                boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
                runPlanner_ = false;
                lock.unlock();
                ROS_DEBUG_NAMED("move_base_recovery", "Something should abort after this.");
                if (recovery_trigger_ == CONTROLLING_R)
                {
                    ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
                    as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
                }
                else if (recovery_trigger_ == PLANNING_R)
                {
                    ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
                    as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
                }
                else if (recovery_trigger_ == OSCILLATION_R)
                {
                    ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
                    as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
                }
                resetState();
                return true;
            }
            break;
        default:
            ROS_ERROR("This case should never be reached, something is wrong, aborting");
            resetState();
            //disable the planner thread 关闭全局规划线程
            boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
            runPlanner_ = false;
            lock.unlock();
            as_->setAborted(move_base_msgs::MoveBaseResult(),
                            "Reached a case that should not be hit in move_base. This is a bug, please report it.");
            return true;
    }
    //we aren't done yet 走到这里代表还没完成
    return false;
}

bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node)
{
    XmlRpc::XmlRpcValue behavior_list;
    if (node.getParam("recovery_behaviors", behavior_list))
    {
        // 参数列表是否是数组数据类型
        if (behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
        {
            for (int i = 0; i < behavior_list.size(); ++i)
            {
                // 参数列表是否是结构数据类型
                if (behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct)
                {
                    if (behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type"))
                    {
                        // check for recovery behaviors with the same name 检查具有相同名称的恢复行为
                        for (int j = i + 1; j < behavior_list.size(); j++)
                        {
                            if (behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct)
                            {
                                if (behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type"))
                                {
                                    std::string name_i = behavior_list[i]["name"];
                                    std::string name_j = behavior_list[j]["name"];
                                    if (name_i == name_j)
                                    {
                                        ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. "
                                                  "Using the default recovery behaviors instead.", name_i.c_str());
                                        return false;
                                    }
                                }
                            }
                        }
                    }
                    else
                    {
                        ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead.");
                        return false;
                    }
                }
                else
                {
                    ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.",
                            behavior_list[i].getType());
                    return false;
                }
            }
            // if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors
            // 如果我们已经做到了这一点，我们知道列表是合法的，所以我们将创建所有的恢复行为
            for (int i = 0; i < behavior_list.size(); ++i)
            {
                try
                {
                    // check if a non fully qualified name has potentially been passed in
                    // 检查是否传入了非完全限定名
                    if (!recovery_loader_.isClassAvailable(behavior_list[i]["type"]))
                    {
                        std::vector <std::string> classes = recovery_loader_.getDeclaredClasses();
                        for (unsigned int i = 0; i < classes.size(); ++i)
                        {
                            if (behavior_list[i]["type"] == recovery_loader_.getName(classes[i]))
                            {
                                // if we've found a match... we'll get the fully qualified name and break out of the loop
                                ROS_WARN("Recovery behavior specifications should now include the package name. "
                                         "You are using a deprecated API. Please switch from %s to %s in your yaml file.",
                                        std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str());
                                behavior_list[i]["type"] = classes[i];
                                break;
                            }
                        }
                    }
                    boost::shared_ptr <nav_core::RecoveryBehavior> behavior(
                            recovery_loader_.createInstance(behavior_list[i]["type"]));
                    //shouldn't be possible, but it won't hurt to check
                    if (behavior.get() == NULL)
                    {
                        ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen");
                        return false;
                    }
                    //initialize the recovery behavior with its name
                    behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_);
                    recovery_behavior_names_.push_back(behavior_list[i]["name"]);
                    recovery_behaviors_.push_back(behavior);
                }
                catch (pluginlib::PluginlibException &ex)
                {
                    ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what());
                    return false;
                }
            }
        }
        else
        {
            ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. "
                      "We'll use the default recovery behaviors instead.", behavior_list.getType());
            return false;
        }
    }
        // if no recovery_behaviors are specified, we'll just load the defaults
        // 如果没有指定recovery_behaviors，我们将只加载默认值
    else return false;
    // if we've made it here... we've constructed a recovery behavior list successfully
    // 如果我们来到这里…我们已经成功地构建了一个恢复行为列表
    return true;
}

// we'll load our default recovery behaviors here 加载默认的恢复行为
void MoveBase::loadDefaultRecoveryBehaviors()
{
    recovery_behaviors_.clear();
    try
    {
        //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility
        ros::NodeHandle n("~");
        n.setParam("conservative_reset/reset_distance", conservative_reset_dist_);
        n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4);
        // first, we'll load a recovery behavior to clear the costmap
        // step 1 加载清理代价地图的恢复行为
        boost::shared_ptr <nav_core::RecoveryBehavior> cons_clear(
                recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
        cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
        recovery_behavior_names_.emplace_back("conservative_reset");
        recovery_behaviors_.push_back(cons_clear);
        // step 2 加载原地旋转的恢复行为
        // next, we'll load a recovery behavior to rotate in place
        boost::shared_ptr <nav_core::RecoveryBehavior> rotate(
                recovery_loader_.createInstance("rotate_recovery/RotateRecovery"));
        if (clearing_rotation_allowed_)
        {
            rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_);
            recovery_behavior_names_.emplace_back("rotate_recovery");
            recovery_behaviors_.push_back(rotate);
        }
        // step 3 加载比较主动积极的代价地图重置行为
        // next, we'll load a recovery behavior that will do an aggressive reset of the costmap
        boost::shared_ptr <nav_core::RecoveryBehavior> ags_clear(
                recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
        ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
        recovery_behavior_names_.emplace_back("aggressive_reset");
        recovery_behaviors_.push_back(ags_clear);
        // step 4 再来一次原地旋转
        // we'll rotate in-place one more time
        if (clearing_rotation_allowed_)
        {
            recovery_behavior_names_.emplace_back("rotate_recovery");
            recovery_behaviors_.push_back(rotate);
        }
    }
    catch (pluginlib::PluginlibException &ex)
    {
        ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what());
    }
}

void MoveBase::resetState()
{
    // Disable the planner thread 关闭全局规划线程
    boost::unique_lock <boost::recursive_mutex> lock(planner_mutex_);
    runPlanner_ = false;
    lock.unlock();
    // Reset statemachine 重置状态机并停止机器人运动
    state_ = PLANNING;
    recovery_index_ = 0;
    recovery_trigger_ = PLANNING_R;
    publishZeroVelocity();
    // if we shutdown our costmaps when we're deactivated... we'll do that now 关闭代价地图
    if (shutdown_costmaps_)
    {
        ROS_DEBUG_NAMED("move_base", "Stopping costmaps");
        planner_costmap_ros_->stop();
        controller_costmap_ros_->stop();
    }
}

// 获取机器人的位姿，获取的过程不复杂，但是注意这里会检测各种可能异常，并抛出异常。是一个很重要的导航模块是否正常的检测过程
bool MoveBase::getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROS *costmap)
{
    geometry_msgs::PoseStamped robot_pose;
    // 将tf2 Transform类型转换为等效的geometry_msgs Pose消息。
    tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
    tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
    robot_pose.header.frame_id = robot_base_frame_;
    robot_pose.header.stamp = ros::Time(); // latest available
    ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
    // get robot pose on the given costmap frame 从代价地图中获得机器人位姿
    try
    {
        tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
    }
    catch (tf2::LookupException &ex)
    {
        ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ConnectivityException &ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    catch (tf2::ExtrapolationException &ex)
    {
        ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
        return false;
    }
    // check if global_pose time stamp is within costmap transform tolerance
    // 检查global_pose时间戳是否在costmap转换容忍度之内
    if (!global_pose.header.stamp.isZero() &&
        current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
    {
        ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. Current time: %.4f, pose stamp: %.4f, tolerance: %.4f",
                costmap->getName().c_str(), current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
        return false;
    }
    return true;
}

};
